package com.wintrisstech.iaroc.irobot;

import java.io.IOException;

import com.sun.squawk.util.UnexpectedException;

public class IRobotCreateInterface {
    protected IRobotCreateSerialConnection serialConnection;
    protected IRobotCreateSensorEventHandler sensorEventHandler;
    protected IRobotCreateSensorState sensorState;

    public IRobotCreateInterface() throws IOException {
        this(null, null);
    }
    
    public IRobotCreateInterface(IRobotCreateSerialConnection serialConnection, IRobotCreateSensorState sensorState) throws IOException {
        if (serialConnection == null) {
            try {
                serialConnection = (IRobotCreateSerialConnection) Class.forName("com.wintrisstech.iaroc.sunspot.SunSpotIRobotCreateSerialConnection").newInstance();
            } catch (InstantiationException e) {
                throw new UnexpectedException(e);
            } catch (IllegalAccessException e) {
                throw new UnexpectedException(e);
            } catch (ClassNotFoundException e) {
                throw new UnexpectedException(e);
            }
        }
        this.serialConnection = serialConnection;
        // Assume the recorded bytes array is the size of the maximum command that can be sent to roomba
//        int[] bytes = serialConnection.getRecordedBytes();
//        for (int i=0; i < bytes.length; i++) {
//            bytes[i] = IRobotCreateConstants.COMMAND_START;
//        }
//        serialConnection.sendBytes(bytes, 0, bytes.length);
//        serialConnection.sendByte(IRobotCreateConstants.COMMAND_RESET);
//        serialConnection.sleep(2000);
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_START);
        serialConnection.afterCommandPause();
//        serialConnection.sendByte(IRobotCreateConstants.COMMAND_MODE_CONTROL);
//        serialConnection.afterCommandPause();
        serialConnection.flushRead();
        full();
        if (sensorState == null) {
            this.sensorState = new IRobotCreateSensorState();
        } else {
            this.sensorState = sensorState;
        }
        leds(true, true, true);
    }

    public void cover() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("cover");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DEMO_COVER);
        serialConnection.afterCommandPause();
    }
    
    public void coverAndDock() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("coverAndDock");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DEMO_COVER_AND_DOCK);
        serialConnection.afterCommandPause();
    }
    
    public void demo(int demoType) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("demo: ");
            System.out.println(demoType);
        }
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DEMO);
        serialConnection.sendByte(demoType);
        serialConnection.afterCommandPause();
    }

    public void digitalOutputs(boolean pin0High, boolean pin1High, boolean pin2High) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DIGITAL_OUTPUTS);
        serialConnection.sendByte((pin0High ? IRobotCreateConstants.DIGITAL_OUTPUT_PIN0:0) | (pin1High ? IRobotCreateConstants.DIGITAL_OUTPUT_PIN1:0) | (pin2High ? IRobotCreateConstants.DIGITAL_OUTPUT_PIN2:0));
        serialConnection.afterCommandPause();
    }
    
    public void drive(int velocity, int radius) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("drive velocity: ");
            System.out.print(velocity);
            System.out.print(" radius: ");
            System.out.println(radius);
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DRIVE);
        serialConnection.sendSignedWord(velocity);
        serialConnection.sendSignedWord(radius);
        serialConnection.afterCommandPause();
    }

    public void driveDirect(int rightVelocity, int leftVelocity) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("driveDirect: ");
            System.out.print(rightVelocity);
            System.out.print(" leftVelocity: ");
            System.out.println(leftVelocity);
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DRIVE_DIRECT);
        serialConnection.sendSignedWord(rightVelocity);
        serialConnection.sendSignedWord(leftVelocity);
        serialConnection.afterCommandPause();
    }
    
    public IRobotCreateSensorEventHandler getSensorEventHandler() {
        return sensorEventHandler;
    }
    public IRobotCreateSensorState getSensorState() {
        return sensorState;
    }
    
    public IRobotCreateSerialConnection getSerialConnection() {
        return serialConnection;
    }
    
    public void leds(boolean powerLedOn, boolean playLedOn, boolean advanceLedOn) throws IOException {
        leds(0, powerLedOn?255:0, playLedOn, advanceLedOn);
    }

    public void leds(int powerColor, int powerIntensity, boolean playLedOn, boolean advanceLedOn) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("leds: play led-");
            System.out.print(playLedOn);
            System.out.print(" advance led-");
            System.out.print(advanceLedOn);
            System.out.print(" power color: ");
            System.out.print(powerColor);
            System.out.print(" power intensity: ");
            System.out.println(powerIntensity);
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_LEDS);
        serialConnection.sendByte((advanceLedOn?8:0) | (playLedOn?2:0));
        serialConnection.sendByte(powerColor);
        serialConnection.sendByte(powerIntensity);
        serialConnection.afterCommandPause();
    }

    public void full() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("full");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_MODE_FULL);
        serialConnection.afterCommandPause();
    }

    public void safe() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("safe");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_MODE_SAFE);
        serialConnection.afterCommandPause();
    }

    public void lowSideDrivers(boolean lowSideDriver0On, boolean lowSideDriver1On, boolean lowSideDriver2On) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_LOW_SIDE_DRIVERS);
        serialConnection.sendByte((lowSideDriver0On ? IRobotCreateConstants.LOW_SIDE_DRIVER0:0) | (lowSideDriver1On ? IRobotCreateConstants.LOW_SIDE_DRIVER1:0) | (lowSideDriver2On ? IRobotCreateConstants.LOW_SIDE_DRIVER2:0));
        serialConnection.afterCommandPause();
    }

    public void pauseResumeStream(boolean pause) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_PAUSE_RESUME_STREAM);
        serialConnection.sendByte(pause?0:1);
    }
    
    public int playScript() throws IOException {
        if (!serialConnection.isRecording()) {
            throw new IllegalStateException("No bytes recorded");
        }
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("playScript");
        };
        int count = serialConnection.stopRecording();
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_SCRIPT);
        serialConnection.sendByte(count);
        serialConnection.sendBytes(serialConnection.getRecordedBytes(), 0, count);
        serialConnection.afterCommandPause();
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_PLAY_SCRIPT);
        serialConnection.afterCommandPause();
        return count;
    }
    
    public void pwmLowSideDrivers(int lowSideDriver0DutyCycle, int lowSideDriver1DutyCycle, int lowSideDriver2DutyCycle) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_PWM_LOW_SIDE_DRIVERS);
        serialConnection.sendByte(lowSideDriver2DutyCycle);
        serialConnection.sendByte(lowSideDriver1DutyCycle);
        serialConnection.sendByte(lowSideDriver0DutyCycle);
        serialConnection.afterCommandPause();
    }
    
    public void queryList(int[] packetIds, IRobotCreateSensorEventHandler eventHandler) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("queryList:");
            for (int i=0; i < packetIds.length; i++) {
                System.out.print(", ");
                System.out.print(packetIds[i]);
            }
            System.out.println();
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_QUERY_LIST);
        serialConnection.sendByte(packetIds.length);
        serialConnection.sendBytes(packetIds, 0, packetIds.length);
        serialConnection.afterCommandPause();
        sensorState.update(packetIds, serialConnection, eventHandler);
    }
    
    public void sendIr(int byteValue) {
        if (IRobotCreateConstants.DEBUG) {
        };
    }
    
    public void sensors(int packetId) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("sensors: ");
            System.out.println(packetId);
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_SENSORS);
        serialConnection.sendByte(packetId);
        serialConnection.afterCommandPause();
        sensorState.update(packetId, serialConnection, sensorEventHandler);
    }
    
    public void setSensorEventHandler(IRobotCreateSensorEventHandler eventHandler) {
        sensorEventHandler = eventHandler;
    }
    
    public int[] showScript() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("showScript");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_SHOW_SCRIPT);
        serialConnection.afterCommandPause();
        int size = serialConnection.readUnsignedByte();
        int[] script = new int[size];
        serialConnection.readUnsignedBytes(script, 0, size);
        return script;
    }
    
    public void song(int songNumber, int[] notes) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_SONG);
        serialConnection.sendByte(notes.length);
        serialConnection.sendBytes(notes, 0, notes.length);
        serialConnection.afterCommandPause();
    }

    public void spot() throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("spot");
        };
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_DEMO_SPOT_COVER);
        serialConnection.afterCommandPause();
    }
    
    public void startRecordingScript() {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("startRecordingScript");
        };
        serialConnection.startRecording();
    }

    public int stopRecordingScript() {
        if (IRobotCreateConstants.DEBUG) {
            System.out.println("stopRecordingScript");
        };
        return serialConnection.stopRecording();
    }
    
    public void stream(int[] packetTypes) throws IOException {
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_STREAM);
        serialConnection.sendByte(packetTypes.length);
        serialConnection.sendBytes(packetTypes, 0, packetTypes.length);
        serialConnection.afterCommandPause();
    }

    public void waitAngle(int degrees) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("waitAngle: ");
            System.out.println(degrees);
        };
        // TODO
        // When create turns right, it increments the angle parameter passed in
        // Will keep turning until the angle variable goes to 0
        boolean doScript = false;
        if (!serialConnection.isRecording) {
            doScript = true;
            startRecordingScript();
        }
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_WAIT_ANGLE);
        serialConnection.sendByte(degrees >> 8);
        serialConnection.sendByte(degrees);
        if (doScript) {
            playScript();
        }
    }

    public void waitDistance(int mms) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("waitDistance: ");
            System.out.println(mms);
        };
        boolean doScript = false;
        if (!serialConnection.isRecording()) {
            doScript = true;
            startRecordingScript();
        }
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_WAIT_DISTANCE);
        serialConnection.sendByte(mms >> 8);
        serialConnection.sendByte(mms & 0xFF);
        if (doScript) {
            playScript();
        }
    }

    public void waitEvent(int event) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("waitEvent: ");
            System.out.println(event);
        };
        boolean doScript = false;
        if (!serialConnection.isRecording()) {
            doScript = true;
            startRecordingScript();
        }
        serialConnection.sendByte(IRobotCreateConstants.COMMAND_WAIT_EVENT);
        serialConnection.sendByte(event);
        if (doScript) {
            playScript();
        }
    }

    public void waitEventNot(int event) throws IOException {
        waitEvent(-event);
    }

    public void waitTimeMilliseconds(int ms) throws IOException {
        if (IRobotCreateConstants.DEBUG) {
            System.out.print("waitTimeMilliseconds: ");
            System.out.println(ms);
        };
        if (serialConnection.isRecording()) {
            serialConnection.sendByte(IRobotCreateConstants.COMMAND_WAIT_TIME);
            // TODO: Max 255 * 100 ms
            serialConnection.sendByte(ms / 100);
        } else {
            serialConnection.sleep(ms);
        }
    }

    public void waitTimeSeconds(double seconds) throws IOException {
        int milliseconds = (int) (seconds * 1000);
        waitTimeMilliseconds(milliseconds);
    }

}